/**************************************************************************************************
* Title: SumQuaternion.cpp
* Author: Gael Huber
* Description: Implementation for various quaternion functions
**************************************************************************************************/
#include "SumMath.h"

/**************************************************************************************************
* Compute a quaternion's axis and angle of rotation (expects unit quaternions)
**************************************************************************************************/
void QuaternionToAxisAngle(const Quaternion* pQ, Vector3* pAxis, float* pAngle) {
	*pAngle = acos(pQ->w) * 2.0f;
	float s = 1.0f / sin(*pAngle * 0.5f);
	pAxis->x = pQ->x * s;
	pAxis->y = pQ->y * s;
	pAxis->z = pQ->z * s;
}

/**************************************************************************************************
* Build a quaternion from a rotation matrix
**************************************************************************************************/
Quaternion* QuaternionRotationMatrix(Quaternion* pOut, const Matrix* pM) {
	// If largest diagonal is M11
	if(pM->_11 > pM->_22 && pM->_11 > pM->_33) {
		pOut->x = pM->_11 - pM->_22 - pM->_33 + 1.0f;
		pOut->y = pM->_12 + pM->_21;
		pOut->z = pM->_13 + pM->_31;
		pOut->w = pM->_32 - pM->_23;
	} else if(pM->_22 > pM->_11 && pM->_22 > pM->_33) {
		pOut->x = pM->_22 - pM->_11 - pM->_33 + 1.0f;
		pOut->y = pM->_12 + pM->_21;
		pOut->z = pM->_23 + pM->_32;
		pOut->w = pM->_13 - pM->_31;
	} else {
		pOut->x = pM->_33 - pM->_11 - pM->_22 + 1.0f;
		pOut->y = pM->_13 + pM->_31;
		pOut->z = pM->_32 + pM->_23;
		pOut->w = pM->_21 - pM->_12;
	}

	return pOut;
}

/**************************************************************************************************
* Rotation about an arbitrary axis
**************************************************************************************************/
Quaternion* QuaternionRotationAxis(Quaternion* pOut, const Vector3* pV, float angle) {
	float s = sin(angle * 0.5f);
	pOut->x = s * pV->x;
	pOut->y = s * pV->y;
	pOut->z = s * pV->z;
	pOut->w = cos(angle * 0.5f);
	return pOut;
}

/**************************************************************************************************
* Yaw around the Y axis, a pitch around the X axis, a roll around the Z axis
**************************************************************************************************/
Quaternion* QuaternionRotationYawPitchRoll(Quaternion* pOut, float yaw, float pitch, float roll) {
	Quaternion q1, q2, q3;

	// Calculate yaw, and pitch
	QuaternionRotationAxis(&q1, &Vector3(0.0f, 1.0f, 0.0f), yaw);
	QuaternionRotationAxis(&q2, &Vector3(1.0f, 0.0f, 0.0f), pitch);
	QuaternionMultiply(&q3, &q1, &q2);

	// Calculate roll
	QuaternionRotationAxis(&q1, &Vector3(0.0f, 0.0f, 1.0f), roll);
	QuaternionMultiply(pOut, &q3, &q1);

	// Return the result
	return pOut;
}

/**************************************************************************************************
* Quaternion multiplication (Q2 * Q1)
**************************************************************************************************/
Quaternion* QuaternionMultiply(Quaternion* pOut, const Quaternion* pQ1, const Quaternion* pQ2) {
	pOut->x = pQ2->w*pQ1->x + pQ2->x*pQ1->w + pQ2->z*pQ1->y - pQ2->y*pQ1->z;
	pOut->y = pQ2->w*pQ1->y + pQ2->y*pQ1->w + pQ2->x*pQ1->z - pQ2->z*pQ1->x;
	pOut->z = pQ2->w*pQ1->z + pQ2->z*pQ1->w + pQ2->y*pQ1->x - pQ2->x*pQ1->y;
	pOut->w = pQ2->w*pQ1->w - pQ2->x*pQ1->x - pQ2->y*pQ1->y - pQ2->z*pQ1->z;
	return pOut;
}

/**************************************************************************************************
* Multiply a quaternion by a Vector3
**************************************************************************************************/
Vector3* QuaternionMultiply(Vector3* pOut, const Quaternion* pQ, const Vector3* pV) {
	Vector3 qVec(pQ->x, pQ->y, pQ->z);
	Vector3 uv, uuv;
	Vec3Cross(&uv, &qVec, pV);
	Vec3Cross(&uuv, &qVec, &uv);
	Vec3Scale(&uv, &uv, 2.0f * pQ->w);
	Vec3Scale(&uuv, &uuv, 2.0f);
	Vec3Add(pOut, pV, &uv);
	Vec3Add(pOut, pOut, &uuv);
	return pOut;
}

/**************************************************************************************************
* Normalize
**************************************************************************************************/
Quaternion* QuaternionNormalize(Quaternion* pOut, const Quaternion* pQ) {
	float l = 1.0f / QuaternionLength(pQ);
	pOut->x = pQ->x * l;
	pOut->y = pQ->y * l;
	pOut->z = pQ->z * l;
	pOut->w = pQ->w * l;
	return pOut;
}

/**************************************************************************************************
* Conjugate and re-norm
**************************************************************************************************/
Quaternion* QuaternionInverse(Quaternion* pOut, const Quaternion* pQ) {
	QuaternionConjugate(pOut, pQ);
	QuaternionNormalize(pOut, pOut);
	return pOut;
}

/**************************************************************************************************
* Expects unit quaternions. If q = (cos(theta), sin(theta) * v); ln(q) = (0, theta * v)
**************************************************************************************************/
Quaternion* QuaternionLn(Quaternion* pOut,  const Quaternion* pQ) {
	// Using above assumption, solve for theta
	float theta(acosf(pQ->w));
	float s(theta / sinf(theta));

	// Set new quaternion
	pOut->x = pQ->x * s;
	pOut->y = pQ->y * s;
	pOut->z = pQ->z * s;
	pOut->w = 0.0f;

	return pOut;
}

/**************************************************************************************************
* Expects pure quaternions. (w == 0) w is ignored in calculation. 
* If q = (0, theta * v); exp(q) = (cos(theta), sin(theta) * v)
**************************************************************************************************/
Quaternion* QuaternionExp(Quaternion* pOut, const Quaternion* pQ) {
	float length(QuaternionLength(pQ));

	if(length > 0.0f) {
		pOut->w = cosf(length);
		float s(sinf(length) / length);
		pOut->x = pQ->x * s;
		pOut->y = pQ->y * s;
		pOut->z = pQ->z * s;
	} else {
		pOut->x = pOut->y = pOut->z = pOut->w = 0.0f;
	}
	return pOut;
}

/**************************************************************************************************
* Spherical linear interpolation between Q1 (t == 0) and Q2 (t == 1). Expects unit quaternions
**************************************************************************************************/
Quaternion* QuaternionSlerp(Quaternion* pOut, const Quaternion* pQ1, const Quaternion* pQ2, float t) {
	// Compute the difference between Q1 and Q2 (dQ)
	Quaternion q1;
	QuaternionInverse(&q1, pQ1);
	Quaternion q2;
	QuaternionMultiply(&q2, &q1, pQ2);
	
	// Take a fraction t of dQ through exponentiation
	float theta = acosf(q2.w);
	float newTheta = theta * t;
	float mult = sinf(newTheta) / sinf(theta);
	q2.w = cosf(newTheta);
	q2.x *= mult;
	q2.y *= mult;
	q2.z *= mult;

	// Modify the original by the fraction
	QuaternionMultiply(pOut, pQ1, &q2);

	return pOut;
}

/**************************************************************************************************
* Spherical quadrangle interpolation (Slerp(Slerp(Q1, C, t), Slerp(A, B, t), 2t(1-t))
**************************************************************************************************/
Quaternion* QuaternionSquad(Quaternion* pOut, const Quaternion* pQ1, const Quaternion* pA, 
	const Quaternion* pB, const Quaternion* pC, float t) {
	Quaternion s1, s2;
	QuaternionSlerp(&s1, pQ1, pC, t);
	QuaternionSlerp(&s2, pA, pB, t);
	QuaternionSlerp(pOut, &s1, &s2, 2.0f * t * (1.0f - t));
	return pOut;
}

/**************************************************************************************************
* Setup control points for spherical quadrangle interpolation from Q1 to Q2.  The control points 
* are chosen in such a way to ensure the continuity of tangents with adjacent segments.
**************************************************************************************************/
void QuaternionSquadSetup(Quaternion* pAOut, Quaternion* pBOut, Quaternion* pCOut, 
	const Quaternion* pQ0, const Quaternion* pQ1, const Quaternion* pQ2, const Quaternion* pQ3 ) {
	Quaternion q0(QuaternionLength(&(*pQ0 + *pQ1)) < QuaternionLength(&(*pQ0 - *pQ1)) ? -*pQ0 : *pQ0);
	Quaternion q1(*pQ1);
	Quaternion q2(QuaternionLength(&(*pQ1 + *pQ2)) < QuaternionLength(&(*pQ1 - *pQ2)) ? -*pQ2 : *pQ2);
	Quaternion q3(QuaternionLength(&(*pQ2 + *pQ3)) < QuaternionLength(&(*pQ2 - *pQ3)) ? -*pQ3 : *pQ3);

	Quaternion qE;
	Quaternion qLN1, qLN2;
	Quaternion t;

	// Calculate A
	QuaternionExp(&qE, pQ1);
	QuaternionLn(&qLN1, &(qE*q2));
	QuaternionLn(&qLN2, &(qE*q0));
	QuaternionExp(&t, &(-0.25f * (qLN1 + qLN2)));
	QuaternionMultiply(pAOut, &q1, &t);

	// Calculate B
	QuaternionExp(&qE, pQ2);
	QuaternionLn(&qLN1, &(qE*q3));
	QuaternionLn(&qLN2, &(qE*q1));
	QuaternionExp(&t, &(-0.25f * (qLN1 + qLN2)));
	QuaternionMultiply(pBOut, &q2, &t);
	
	// Calculate C
	*pCOut = q2;
}

/**************************************************************************************************
* Barycentric interpolation. Slerp(Slerp(Q1, Q2, f+g), Slerp(Q1, Q3, f+g), g/(f+g))
**************************************************************************************************/
Quaternion* QuaternionBarycentric(Quaternion* pOut, const Quaternion* pQ1, const Quaternion* pQ2, 
	const Quaternion* pQ3, float f, float g) {
	// Internal Slerps
	Quaternion q1, q2;
	QuaternionSlerp(&q1, pQ1, pQ2, f + g);
	QuaternionSlerp(&q2, pQ1, pQ3, f + g);

	// External Slerp
	QuaternionSlerp(pOut, &q1, &q2, g / (f + g));
	
	return pOut;
}